#pragma once
#include <cstdint>
#include <vector>
#include <memory>
#include "zuc/nml_intf/zucpos2.h"
#include "zuc/mot_cfg.h"

namespace Util {
class IniParser;
}

namespace axisgroup {

class UsrFrame
{
public:
    UsrFrame() {}
    void set_pose(const ZucPose& pose);
    //在世界坐标系下的位置。
    const ZucPose& world(int ref_id = 0);  // 相对于ref_id的坐标系
    void config_name(std::string name) { name_ = name; }
    const ZucPose& pose() const { return pose_; }
    std::string pose_str() const;

private:
    int id_;
    bool is_changed_ = false;
    std::string name_;
    ZucPose pose_;               // 当前用户坐标系
    ZucPose world_pose_;         // 当前相对于基座标系的坐标系
    uint32_t ref_frame_id_ = 0;  //注意：ref_frame_id仅允许两层嵌套，即ref_frame_id的frame的ref_frame_id默认为0（世界坐标系）

    friend class UsrFrameMgr;
};

class UsrFrameMgr
{
public:
    UsrFrameMgr() = delete;
    const static uint32_t MAX_USR_FRAME_NUM = kUsrFrameMaxCnt;  // 0-16为共用的user_frame; 17为1机器人的base_frame,18为2机器人的base_frame
    static UsrFrame& get_usr_frame(uint32_t id);
    static int init_usr_frame(const char* filename);
    static void save();

private:
    int id_ = 0;
    static std::unique_ptr<Util::IniParser> ini_parser_;
    static UsrFrame __usr_frame[MAX_USR_FRAME_NUM];
};

}  // namespace axisgroup
